#include <cmath>
#include <Eigen/Dense>
#include <gsl/gsl_errno.h>
#include "ellipsoid_no_slip.h"

namespace Mechanics { namespace Rattleback { namespace Ellipsoid {
namespace NoSlip {
  int ode(double t, const double x[], double dxdt[], void *params)
  {
    using namespace Eigen;
    parameters *p = static_cast<parameters *>(params);
    const double a = p->a,
                        b = p->b,
                        c = p->c,
                        d = p->d,
                        e = p->e,
                        f = p->f,
                        m = p->m,
                        Ixx = p->Ixx,
                        Iyy = p->Iyy,
                        Izz = p->Izz,
                        Ixy = p->Ixy,
                        Iyz = p->Iyz,
                        Ixz = p->Ixz,
                        g = p->g,
                        s = p->s;
    double z[83];
    Matrix3d M_dyn;
    Vector3d f_dyn;
    Vector3d udot;
    FullPivHouseholderQR<Matrix3d> dec;

    // Intermediate variables for ODE function
    z[0] = sin(x[2]);
    z[1] = cos(x[2]);
    z[2] = pow(c, 2);
    z[3] = cos(x[1]);
    z[4] = sin(x[1]);
    z[5] = pow(a, 2);
    z[6] = pow(b, 2);
    z[7] = cos(x[0]);
    z[8] = sin(x[0]);
    z[9] = -x[1 + 5];
    z[10] = -x[2 + 5];
    z[11] = -x[0 + 5];
    z[12] = g*m;
    z[13] = x[0 + 5]*z[0];
    z[14] = x[2 + 5]*z[1];
    z[15] = pow(z[3], 2);
    z[16] = pow(z[1], 2);
    z[17] = pow(z[0], 2);
    z[18] = -z[0];
    dxdt[0] = (-z[13] + z[14])/sqrt(z[15]);
    dxdt[1] = x[0 + 5]*z[1] + x[2 + 5]*z[0];
    dxdt[2] = x[1 + 5] + (z[13] - z[14])*tan(x[1]);
    z[19] = dxdt[1]*z[4];
    z[20] = dxdt[1]*z[3];
    z[21] = z[1]*z[7];
    z[22] = z[1]*z[5];
    z[23] = z[0]*z[4];
    z[24] = z[3]*z[5];
    z[25] = z[1]*z[8];
    z[26] = z[1]*z[3];
    z[27] = z[16]*z[2];
    z[28] = Ixz*x[0 + 5] + Iyz*x[1 + 5] + Izz*x[2 + 5];
    z[29] = Ixy*x[0 + 5] + Iyy*x[1 + 5] + Iyz*x[2 + 5];
    z[30] = Ixx*x[0 + 5] + Ixy*x[1 + 5] + Ixz*x[2 + 5];
    z[31] = dxdt[2]*z[0]*z[15];
    z[32] = z[15]*z[17]*z[5] + z[15]*z[27] + pow(z[4], 2)*z[6];
    z[33] = pow(z[32], -1.0/2.0);
    z[34] = pow(z[33], 3);
    z[35] = z[33]*z[5];
    z[36] = z[2]*z[33];
    z[37] = z[33]*z[6];
    z[38] = dxdt[2]*z[3]*z[33];
    z[39] = e - z[37]*z[4];
    z[40] = z[37]*z[4];
    z[41] = -e + z[40];
    z[42] = x[0 + 5]*z[39];
    z[43] = m*z[41];
    z[44] = m*z[39];
    z[45] = -f + z[26]*z[36];
    z[46] = z[26]*z[36];
    z[47] = f - z[46];
    z[48] = z[10]*z[39];
    z[49] = d + z[0]*z[24]*z[33];
    z[50] = -z[49];
    z[51] = dxdt[2]*z[0]*z[3]*z[36];
    z[52] = z[1]*z[19]*z[36];
    z[53] = x[1 + 5]*z[47];
    z[54] = m*z[47];
    z[55] = z[50];
    z[56] = x[2 + 5]*z[49];
    z[57] = m*z[49];
    z[58] = z[49]*z[9];
    z[59] = z[1]*z[2]*z[31] + z[17]*z[19]*z[24] + z[19]*z[27]*z[3] - z[19]*z[3]*z[6] - z[22]*z[31];
    z[60] = -Iyz - z[43]*z[47];
    z[61] = -Ixy - z[44]*z[50];
    z[62] = z[45]*z[9] + z[48];
    z[63] = z[51] + z[52];
    z[64] = z[11]*z[41] + z[58];
    z[65] = -Ixz - z[45]*z[57];
    z[66] = z[10]*z[55] + z[11]*z[47];
    z[67] = z[34]*z[59];
    z[68] = -z[67];
    z[69] = z[2]*z[67];
    z[70] = z[4]*z[6]*z[67];
    z[71] = z[26]*z[69];
    z[72] = z[20]*z[37] + z[70];
    z[73] = -z[72];
    z[74] = -z[20]*z[37] + z[4]*z[6]*z[68];
    z[75] = z[3]*z[72];
    z[76] = z[63] - z[71];
    z[77] = z[0]*z[19]*z[35] + z[18]*z[24]*z[34]*(z[1]*z[2]*z[31] + z[17]*z[19]*z[3]*z[5] + z[19]*z[27]*z[3] - z[19]*z[3]*z[6] - z[22]*z[31]) - z[22]*z[38];
    z[78] = z[0]*z[24]*z[34]*(z[1]*z[2]*z[31] + z[17]*z[19]*z[3]*z[5] + z[19]*z[27]*z[3] - z[19]*z[3]*z[6] - z[22]*z[31]) + z[18]*z[19]*z[35] + z[22]*z[38];
    z[79] = x[2 + 5]*z[78];
    z[80] = z[11]*z[76];
    z[81] = z[78]*z[9];
    z[82] = z[79] + z[80];

    // Kinematic differential equations
    dxdt[3] = -z[75]*z[8] + z[77]*(z[21] - z[23]*z[8]) + (z[0]*z[7] + z[25]*z[4])*(dxdt[2]*z[18]*z[3]*z[36] - z[52] + z[71]);
    dxdt[4] = z[7]*z[75] + z[77]*(z[23]*z[7] + z[25]) + (-z[63] + z[71])*(z[0]*z[8] - z[21]*z[4]);

    // Mass matrix
    M_dyn(0, 0) = -Ixx - m*pow(z[45], 2) - z[39]*z[44];
    M_dyn(0, 1) = z[61];
    M_dyn(0, 2) = z[65];
    M_dyn(1, 0) = z[61];
    M_dyn(1, 1) = -Iyy - m*pow(z[50], 2) - z[47]*z[54];
    M_dyn(1, 2) = z[60];
    M_dyn(2, 0) = z[65];
    M_dyn(2, 1) = z[60];
    M_dyn(2, 2) = -Izz - z[41]*z[43] - z[49]*z[57];

    // Right hand side of dynamic equations
    f_dyn(0) = m*z[45]*(x[2 + 5]*z[62] + z[11]*(z[42] + z[58]) + z[82]) + s*x[0 + 5] + x[1 + 5]*z[28] - x[2 + 5]*z[29] - z[12]*(z[26]*z[39] + z[4]*z[45]) + z[44]*(x[0 + 5]*z[74] + x[0 + 5]*(x[0 + 5]*z[45] + z[56]) + z[81] + z[9]*(-x[1 + 5]*z[45] + z[48]));
    f_dyn(1) = m*z[55]*(x[0 + 5]*z[66] + x[0 + 5]*z[73] + z[81] + z[9]*(z[48] + z[53])) + s*x[1 + 5] - x[0 + 5]*z[28] + x[2 + 5]*z[30] - z[12]*(-z[0]*z[3]*z[47] + z[26]*z[55]) + z[54]*(x[1 + 5]*(x[1 + 5]*z[55] + z[42]) + x[1 + 5]*(z[2]*z[26]*z[68] + z[63]) + z[10]*z[73] + z[10]*(-x[2 + 5]*z[55] + z[11]*z[47]));
    f_dyn(2) = s*x[2 + 5] + x[0 + 5]*z[29] - x[1 + 5]*z[30] - z[12]*(z[18]*z[3]*z[41] + z[4]*z[49]) + z[43]*(x[1 + 5]*z[64] + x[1 + 5]*z[76] + z[10]*z[74] + z[10]*(-x[0 + 5]*z[47] + z[56])) + z[57]*(x[2 + 5]*(x[2 + 5]*z[41] + z[53]) + z[11]*(-x[0 + 5]*z[41] + z[58]) + z[82]);

    // Solve M_dyn*du/dt = f_dyn for du/dt
    dec.compute(M_dyn);
    udot = dec.solve(f_dyn);
    dxdt[5] = udot(0);
    dxdt[6] = udot(1);
    dxdt[7] = udot(2);
    return GSL_SUCCESS;
  }

  int jacobian(double t, const double x[], double dfdx[], double dfdt[], void *params)
  {
    using namespace Eigen;
    parameters *p = static_cast<parameters *>(params);
    const double a = p->a,
                        b = p->b,
                        c = p->c,
                        d = p->d,
                        e = p->e,
                        f = p->f,
                        m = p->m,
                        Ixx = p->Ixx,
                        Iyy = p->Iyy,
                        Izz = p->Izz,
                        Ixy = p->Ixy,
                        Iyz = p->Iyz,
                        Ixz = p->Ixz,
                        g = p->g,
                        s = p->s;
    dfdt[0] = dfdt[1] = dfdt[2] = dfdt[3] = dfdt[4] = dfdt[5] = dfdt[6] = dfdt[7] = 0.0;
    Matrix<double, 8, 8> J; 
    Matrix<double, 3, 3> M_dyn;
    double z[356];
    double dxdt[8];

    // Compute dxdt
    ode(t, x, dxdt, params);

    // Intermediate quantities needed for Jacobian matrix
    z[0] = cos(x[1]);
    z[1] = sin(x[2]);
    z[2] = cos(x[2]);
    z[3] = tan(x[1]);
    z[4] = pow(a, 2);
    z[5] = sin(x[1]);
    z[6] = pow(c, 2);
    z[7] = pow(b, 2);
    z[8] = cos(x[0]);
    z[9] = sin(x[0]);
    z[10] = Izz*x[2 + 5];
    z[11] = Ixy*x[2 + 5];
    z[12] = Iyy*x[1 + 5];
    z[13] = Ixx*x[0 + 5];
    z[14] = Iyz*x[0 + 5];
    z[15] = Ixz*x[1 + 5];
    z[16] = -x[1 + 5];
    z[17] = -x[2 + 5];
    z[18] = -x[0 + 5];
    z[19] = -dxdt[1];
    z[20] = 2*x[2 + 5];
    z[21] = g*m;
    z[22] = 2*x[0 + 5];
    z[23] = 2*x[1 + 5];
    z[24] = -m;
    z[25] = 1.0/z[0];
    z[26] = x[0 + 5]*z[1];
    z[27] = x[2 + 5]*z[2];
    z[28] = pow(z[25], -2);
    z[29] = pow(z[2], 2);
    z[30] = pow(z[1], 2);
    z[31] = pow(z[5], 2);
    z[32] = -z[2];
    z[33] = dxdt[2]*z[6];
    z[34] = 3*z[5];
    z[35] = dxdt[1]*z[7];
    z[36] = dxdt[2]*z[4];
    z[37] = dxdt[1]*z[6];
    z[38] = dxdt[1]*z[4];
    z[39] = dxdt[1]*z[5];
    z[40] = -z[1];
    z[41] = pow(z[3], 2) + 1;
    z[42] = z[1]*z[9];
    z[43] = z[1]*z[3];
    z[44] = z[1]*z[4];
    z[45] = z[0]*z[4];
    z[46] = z[1]*z[6];
    z[47] = z[1]*z[5];
    z[48] = z[5]*z[7];
    z[49] = z[0]*z[1];
    z[50] = z[0]*z[6];
    z[51] = z[0]*z[9];
    z[52] = z[2]*z[3];
    z[53] = z[0]*z[8];
    z[54] = z[2]*z[9];
    z[55] = z[0]*z[2];
    z[56] = z[2]*z[8];
    z[57] = z[2]*z[6];
    z[58] = -z[26] + z[27];
    z[59] = z[26] - z[27];
    z[60] = z[2]*z[28];
    z[61] = x[0 + 5]*z[2] + x[2 + 5]*z[1];
    z[62] = z[5]*z[56];
    z[63] = z[28]*z[30];
    z[64] = z[28]*z[29];
    z[65] = z[3]*z[61];
    z[66] = -z[42]*z[5] + z[56];
    z[67] = z[42] - z[62];
    z[68] = z[41]*z[59];
    z[69] = z[1]*z[8] + z[5]*z[54];
    z[70] = -z[69];
    z[71] = z[47]*z[8] + z[54];
    z[72] = -z[70];
    z[73] = -z[44]*z[60] + z[46]*z[60];
    z[74] = -3*z[44]*z[60] + 3*z[46]*z[60];
    z[75] = z[2]*z[73];
    z[76] = z[7]*z[73];
    z[77] = z[0]*z[73];
    z[78] = z[31]*z[7] + z[4]*z[63] + z[6]*z[64];
    z[79] = -z[0]*z[48] + z[29]*z[5]*z[50] + z[30]*z[45]*z[5];
    z[80] = pow(z[78], -1.0/2.0);
    z[81] = pow(z[80], 3);
    z[82] = pow(z[81], 5.0/3.0);
    z[83] = dxdt[2]*z[81];
    z[84] = z[0]*z[80];
    z[85] = z[5]*z[80];
    z[86] = z[5]*z[81];
    z[87] = z[7]*z[81];
    z[88] = z[2]*z[81];
    z[89] = -z[81];
    z[90] = z[2]*z[79];
    z[91] = z[0]*z[81];
    z[92] = -z[0]*z[34]*z[7] + z[29]*z[34]*z[50] + z[30]*z[34]*z[45];
    z[93] = z[0]*z[89];
    z[94] = e - z[48]*z[80];
    z[95] = z[48]*z[80];
    z[96] = -e + z[95];
    z[97] = z[48]*z[81];
    z[98] = x[0 + 5]*z[94];
    z[99] = x[2 + 5]*z[94];
    z[100] = x[1 + 5]*z[96];
    z[101] = z[35]*z[84];
    z[102] = m*z[96];
    z[103] = 2*z[96];
    z[104] = m*z[94];
    z[105] = -z[94];
    z[106] = z[19]*z[95];
    z[107] = z[57]*z[85];
    z[108] = -f + z[2]*z[50]*z[80];
    z[109] = z[44]*z[85];
    z[110] = z[19]*z[7]*z[84];
    z[111] = z[2]*z[50]*z[80];
    z[112] = f - z[111];
    z[113] = z[17]*z[94];
    z[114] = z[46]*z[84];
    z[115] = z[20]*z[96];
    z[116] = z[38]*z[47]*z[80];
    z[117] = z[38]*z[49]*z[80];
    z[118] = z[2]*z[36]*z[85];
    z[119] = z[33]*z[47]*z[80];
    z[120] = z[37]*z[47]*z[80];
    z[121] = z[36]*z[55]*z[80];
    z[122] = d + z[44]*z[84];
    z[123] = z[33]*z[55]*z[80];
    z[124] = -z[122];
    z[125] = x[2 + 5]*z[112];
    z[126] = z[33]*z[49]*z[80];
    z[127] = z[2]*z[37]*z[85];
    z[128] = x[0 + 5]*z[112];
    z[129] = x[1 + 5]*z[112];
    z[130] = 2*z[112];
    z[131] = m*z[112];
    z[132] = x[0 + 5]*z[108];
    z[133] = m*z[108];
    z[134] = z[111]*z[19];
    z[135] = z[107]*z[19];
    z[136] = z[33]*z[40]*z[84];
    z[137] = x[2 + 5]*z[122];
    z[138] = z[32]*z[36]*z[84];
    z[139] = z[109]*z[19];
    z[140] = z[108]*z[16];
    z[141] = z[112]*z[18];
    z[142] = x[1 + 5]*z[122];
    z[143] = z[19]*z[46]*z[85];
    z[144] = z[36]*z[40]*z[84];
    z[145] = z[19]*z[2]*z[4]*z[85];
    z[146] = z[124];
    z[147] = m*z[122];
    z[148] = z[146];
    z[149] = z[148];
    z[150] = m*z[148];
    z[151] = z[148]*z[17];
    z[152] = m*z[149];
    z[153] = z[73]*z[81];
    z[154] = z[73]*z[86];
    z[155] = z[76]*z[81];
    z[156] = z[74]*z[82];
    z[157] = z[75]*z[81];
    z[158] = z[153]*z[39];
    z[159] = z[73]*z[97];
    z[160] = z[0]*z[156];
    z[161] = x[2 + 5]*z[159];
    z[162] = m*z[159];
    z[163] = z[0]*z[29]*z[37]*z[5] + z[0]*z[30]*z[38]*z[5] - z[0]*z[35]*z[5] + z[1]*z[33]*z[60] - z[1]*z[36]*z[60];
    z[164] = z[157]*z[50];
    z[165] = z[19]*z[76]*z[91];
    z[166] = z[159]*z[18];
    z[167] = z[79]*z[81];
    z[168] = z[153]*z[33]*z[49];
    z[169] = z[37]*z[75]*z[86];
    z[170] = z[153]*z[36]*z[55];
    z[171] = -z[163];
    z[172] = z[159]*z[53];
    z[173] = z[19]*z[44]*z[86]*(-z[1]*z[4]*z[60] + z[46]*z[60]);
    z[174] = z[0]*z[163];
    z[175] = z[163]*z[2];
    z[176] = z[48]*z[51]*z[73]*z[89];
    z[177] = z[82]*z[92];
    z[178] = z[0]*z[171];
    z[179] = z[35]*z[79]*z[91];
    z[180] = z[50]*z[88]*(z[0]*z[29]*z[5]*z[6] - z[0]*z[48] + z[30]*z[45]*z[5]);
    z[181] = z[44]*z[79]*z[91];
    z[182] = z[167]*z[38]*z[47];
    z[183] = z[167]*z[36]*z[55];
    z[184] = z[19]*z[57]*z[79]*z[86];
    z[185] = z[33]*z[40]*z[79]*z[91];
    z[186] = z[113] + z[129];
    z[187] = -z[142] + z[98];
    z[188] = -Iyz - z[102]*z[112];
    z[189] = -z[33]*z[63] + z[33]*z[64] + z[36]*z[63] - z[36]*z[64] - 2*z[37]*z[47]*z[55] + 2*z[38]*z[47]*z[55];
    z[190] = z[113] + z[140];
    z[191] = z[122]*z[16] + z[98];
    z[192] = -Ixy - z[104]*z[124];
    z[193] = z[115] + z[139];
    z[194] = -Ixz - z[122]*z[133];
    z[195] = -z[128] + z[151];
    z[196] = z[141] + z[149]*z[17];
    z[197] = z[163]*z[82];
    z[198] = z[163]*z[81];
    z[199] = z[171]*z[82];
    z[200] = -z[28]*z[35] - z[29]*z[31]*z[37] - z[30]*z[31]*z[38] + z[31]*z[35] - 2*z[33]*z[47]*z[55] + 2*z[36]*z[47]*z[55] + z[37]*z[64] + z[38]*z[63];
    z[201] = z[174]*z[82];
    z[202] = z[175]*z[82];
    z[203] = z[178]*z[82];
    z[204] = z[174]*z[87];
    z[205] = z[163]*z[97];
    z[206] = z[163]*z[48]*z[89];
    z[207] = z[114] - z[164];
    z[208] = z[164] + z[40]*z[50]*z[80];
    z[209] = z[163]*z[44]*z[86];
    z[210] = z[163]*z[44]*z[91];
    z[211] = z[2]*z[45]*z[80] + z[44]*z[91]*(-z[1]*z[4]*z[60] + z[46]*z[60]);
    z[212] = z[114] + z[153]*z[32]*z[50];
    z[213] = z[163]*z[46]*z[91];
    z[214] = z[163]*z[50]*z[88];
    z[215] = -z[211];
    z[216] = z[163]*z[45]*z[88];
    z[217] = z[153]*z[40]*z[45] + z[32]*z[45]*z[80];
    z[218] = z[163]*z[32]*z[6]*z[86];
    z[219] = z[198]*z[40]*z[45];
    z[220] = z[198]*z[32]*z[50];
    z[221] = x[2 + 5]*z[211];
    z[222] = z[7]*z[84] + z[97]*(-z[0]*z[5]*z[7] + z[29]*z[5]*z[50] + z[30]*z[45]*z[5]);
    z[223] = -z[222];
    z[224] = z[18]*z[212];
    z[225] = -z[222];
    z[226] = z[48]*z[89]*(-z[0]*z[5]*z[7] + z[29]*z[5]*z[50] + z[30]*z[45]*z[5]) - z[7]*z[84];
    z[227] = x[0 + 5]*z[223];
    z[228] = x[0 + 5]*z[226];
    z[229] = -z[107] + z[180];
    z[230] = -z[109] + z[181];
    z[231] = z[109] - z[181];
    z[232] = z[109] + z[40]*z[45]*z[81]*(z[0]*z[30]*z[4]*z[5] - z[0]*z[48] + z[29]*z[5]*z[50]);
    z[233] = z[222]*z[53];
    z[234] = z[180] + z[32]*z[6]*z[85];
    z[235] = z[107] + z[32]*z[50]*z[81]*(z[0]*z[29]*z[5]*z[6] - z[0]*z[48] + z[30]*z[45]*z[5]);
    z[236] = z[181] + z[4]*z[40]*z[85];
    z[237] = x[2 + 5]*z[230];
    z[238] = x[1 + 5]*z[235];
    z[239] = z[18]*z[235];
    z[240] = z[208]*z[69];
    z[241] = z[217]*z[66];
    z[242] = z[217]*z[71];
    z[243] = z[189]*z[48]*z[89];
    z[244] = z[189]*z[50]*z[88];
    z[245] = z[189]*z[44]*z[91];
    z[246] = z[156]*z[163];
    z[247] = z[246];
    z[248] = z[189]*z[32]*z[50]*z[81];
    z[249] = z[156]*z[171];
    z[250] = z[160]*z[163];
    z[251] = z[232]*z[71];
    z[252] = z[156]*z[175]*z[50];
    z[253] = z[163]*z[177];
    z[254] = z[253];
    z[255] = z[171]*z[177];
    z[256] = z[174]*z[177];
    z[257] = z[200]*z[97];
    z[258] = z[200]*z[50]*z[88];
    z[259] = z[200]*z[44]*z[91];
    z[260] = z[101] + z[205];
    z[261] = -z[260];
    z[262] = z[110] + z[206];
    z[263] = x[0 + 5]*z[262];
    z[264] = z[260]*z[5];
    z[265] = z[17]*z[262];
    z[266] = z[121] + z[210];
    z[267] = z[143] + z[248];
    z[268] = x[1 + 5]*z[207] + z[161];
    z[269] = -x[0 + 5]*z[159] - x[1 + 5]*z[211];
    z[270] = x[1 + 5]*z[212] + z[161];
    z[271] = x[1 + 5]*z[215] + z[166];
    z[272] = m*z[268];
    z[273] = x[2 + 5]*z[270];
    z[274] = z[16]*z[211] + z[166];
    z[275] = m*z[270];
    z[276] = m*z[269];
    z[277] = z[16]*z[270];
    z[278] = z[128] + z[260];
    z[279] = z[165] + z[243];
    z[280] = z[126] + z[127] - z[214];
    z[281] = z[172] + z[242];
    z[282] = z[135] + z[136] + z[214];
    z[283] = z[116] + z[138] + z[219];
    z[284] = z[139] + z[266];
    z[285] = z[126] + z[127] + z[220];
    z[286] = x[1 + 5]*z[280];
    z[287] = x[1 + 5]*z[285];
    z[288] = x[2 + 5]*z[284];
    z[289] = z[16]*z[284];
    z[290] = z[2]*z[282];
    z[291] = z[102]*z[268];
    z[292] = z[104]*z[269];
    z[293] = -x[0 + 5]*z[207] + z[221];
    z[294] = z[131]*z[270];
    z[295] = z[221] + z[224];
    z[296] = z[17]*z[215] + z[224];
    z[297] = z[183] + z[259];
    z[298] = m*z[295];
    z[299] = z[150]*z[269];
    z[300] = -x[2 + 5]*z[223] + z[238];
    z[301] = z[17]*z[223] + z[238];
    z[302] = z[16]*z[230] + z[228];
    z[303] = z[16]*z[236] + z[227];
    z[304] = m*z[300];
    z[305] = z[17]*z[226] + z[238];
    z[306] = m*z[302];
    z[307] = m*z[303];
    z[308] = -x[0 + 5]*z[235] + z[237];
    z[309] = m*z[308];
    z[310] = z[17]*z[231] + z[239];
    z[311] = x[2 + 5]*z[236] + z[239];
    z[312] = z[233] + z[251];
    z[313] = z[147]*z[293];
    z[314] = z[133]*z[295];
    z[315] = z[117] + z[118] + z[182] + z[209];
    z[316] = z[131]*z[300];
    z[317] = z[133]*z[308];
    z[318] = z[147]*z[311];
    z[319] = z[123] + z[168] + z[169] + z[213];
    z[320] = z[120] + z[244] + z[252];
    z[321] = z[176] + z[240] + z[241];
    z[322] = z[208]*z[67] + z[281];
    z[323] = z[322];
    z[324] = z[189]*z[97] + z[247]*z[48] + z[35]*z[77]*z[81];
    z[325] = z[249]*z[48] + z[279];
    z[326] = x[0 + 5]*z[325];
    z[327] = z[263] + z[289];
    z[328] = z[106] + z[179] + z[204] + z[257];
    z[329] = -z[222]*z[51] + z[229]*z[69] + z[232]*z[66];
    z[330] = z[234]*z[67] + z[312];
    z[331] = z[294] + z[299];
    z[332] = z[144] + z[145] + z[170] + z[173] + z[216] + z[245];
    z[333] = z[292] + z[314];
    z[334] = z[119] + z[134] + z[184] + z[185] + z[218] + z[258];
    z[335] = z[0]*z[19]*z[79]*z[87] + z[178]*z[87] + z[200]*z[48]*z[89] + z[255]*z[48] + z[35]*z[85];
    z[336] = x[0 + 5]*z[335];
    z[337] = z[17]*z[335];
    z[338] = x[0 + 5]*(z[132] + z[137]) + z[16]*(-x[1 + 5]*z[108] + z[113]) + z[327];
    z[339] = m*z[338];
    z[340] = z[150]*z[303] + z[316];
    z[341] = x[2 + 5]*z[190] + z[18]*z[191] + z[18]*z[285] + z[288];
    z[342] = m*z[341];
    z[343] = z[153]*z[38]*z[47] + z[189]*z[40]*z[45]*z[81] + z[198]*z[32]*z[45] + z[2]*z[38]*z[85] + z[247]*z[40]*z[45] + z[32]*z[36]*z[77]*z[81] + z[36]*z[49]*z[80];
    z[344] = z[201]*z[44]*(-3*z[1]*z[4]*z[60] + 3*z[46]*z[60]) + z[332];
    z[345] = z[247]*z[32]*z[50] + z[267] + z[319];
    z[346] = z[345];
    z[347] = z[16]*z[344];
    z[348] = z[163]*z[4]*z[40]*z[86] + z[19]*z[44]*z[79]*z[86] + z[19]*z[44]*z[84] + z[256]*z[44] + z[297] + z[32]*z[36]*z[85];
    z[349] = z[163]*z[57]*z[86] + z[167]*z[33]*z[49] + z[197]*z[32]*z[50]*(z[0]*z[29]*z[34]*z[6] - z[0]*z[34]*z[7] + z[30]*z[34]*z[45]) + z[200]*z[32]*z[50]*z[81] + z[33]*z[40]*z[85] + z[37]*z[55]*z[80] + z[37]*z[86]*z[90];
    z[350] = x[2 + 5]*z[348];
    z[351] = x[1 + 5]*z[349];
    z[352] = z[16]*z[348];
    z[353] = z[277] + z[326] + z[347];
    z[354] = z[336] + z[352];
    z[355] = z[337] + z[351];

    // Entries of Jacobian matrix
    J(0, 0) = 0;
    J(0, 1) = z[5]*z[58]/z[28];
    J(0, 2) = -z[25]*z[61];
    J(0, 3) = 0;
    J(0, 4) = 0;
    J(0, 5) = z[25]*z[40];
    J(0, 6) = 0;
    J(0, 7) = z[2]*z[25];
    J(1, 0) = 0;
    J(1, 1) = 0;
    J(1, 2) = z[58];
    J(1, 3) = 0;
    J(1, 4) = 0;
    J(1, 5) = z[2];
    J(1, 6) = 0;
    J(1, 7) = z[1];
    J(2, 0) = 0;
    J(2, 1) = z[68];
    J(2, 2) = z[65];
    J(2, 3) = 0;
    J(2, 4) = 0;
    J(2, 5) = z[43];
    J(2, 6) = 1;
    J(2, 7) = z[3]*z[32];
    J(3, 0) = -z[260]*z[53] + z[282]*(-z[42] + z[62]) - z[71]*(z[116] - z[266]);
    J(3, 1) = -z[0]*z[283]*z[42] + z[264]*z[9] + z[290]*z[51] + z[321]*z[68] - z[51]*(z[254]*z[48] + z[328]) + z[66]*(z[197]*z[40]*z[45]*(z[0]*z[30]*z[34]*z[4] - z[0]*z[34]*z[7] + z[29]*z[34]*z[50]) + z[200]*z[40]*z[45]*z[81] + z[315] + z[32]*z[36]*z[79]*z[91]) + z[69]*(z[202]*z[50]*(z[0]*z[29]*z[34]*z[6] - z[0]*z[34]*z[7] + z[30]*z[34]*z[45]) + z[334]);
    J(3, 2) = z[282]*z[66] + z[283]*z[70] - z[324]*z[51] + z[343]*z[66] + z[58]*(z[225]*z[51] + z[229]*z[72] + z[232]*z[66]) + z[65]*(z[176] + z[208]*z[72] + z[241]) + z[72]*(-z[319] + z[320]);
    J(3, 3) = 0;
    J(3, 4) = 0;
    J(3, 5) = z[2]*z[329] + z[321]*z[43];
    J(3, 6) = -z[159]*z[51] + z[240] + z[241];
    J(3, 7) = z[1]*z[329] - z[321]*z[52];
    J(4, 0) = -z[260]*z[51] + z[282]*z[69] + z[283]*z[66];
    J(4, 1) = -z[264]*z[8] + z[283]*z[49]*z[8] - z[290]*z[53] + z[323]*z[68] + z[53]*(z[254]*z[48] + z[328]) + z[67]*(z[202]*z[50]*(z[0]*z[29]*z[34]*z[6] - z[0]*z[34]*z[7] + z[30]*z[34]*z[45]) + z[334]) + z[71]*(-z[256]*z[44] - z[297] + z[315]);
    J(4, 2) = z[282]*z[71] - z[283]*z[67] + z[323]*z[65] + z[324]*z[53] + z[330]*z[58] + z[343]*z[71] + z[67]*(z[154]*z[19]*z[57] + z[198]*z[40]*z[50] + z[32]*z[33]*z[84] + z[320] + z[33]*z[40]*z[77]*z[81]);
    J(4, 3) = 0;
    J(4, 4) = 0;
    J(4, 5) = z[2]*z[330] + z[323]*z[43];
    J(4, 6) = z[323];
    J(4, 7) = z[1]*z[330] - z[323]*z[52];
    J(5, 0) = 0;
    J(5, 1) = z[104]*(x[0 + 5]*z[311] + z[16]*z[301] + z[354]) + z[133]*(-x[0 + 5]*z[349] - x[0 + 5]*(-x[1 + 5]*z[236] + z[227]) + x[2 + 5]*z[301] + z[350]) - z[21]*(z[0]*z[108] + z[223]*z[55] + z[234]*z[5] + z[32]*z[5]*z[94]) + z[223]*z[339] + z[234]*z[342] + z[333]*z[68];
    J(5, 2) = z[104]*(x[0 + 5]*z[295] + z[353]) + z[133]*(x[2 + 5]*z[344] + z[18]*z[269] + z[18]*z[346] + z[273]) - z[162]*(x[0 + 5]*(z[110] + z[171]*z[7]*z[86]) + x[0 + 5]*(z[132] + z[137]) + z[16]*(-x[1 + 5]*z[108] + z[17]*(e - z[7]*z[85])) + z[289]) + z[208]*z[342] - z[21]*(z[0]*z[40]*z[94] + z[208]*z[5] + z[32]*z[77]*z[97]) + z[333]*z[65] + z[58]*(z[104]*z[302] + z[317]);
    J(5, 3) = 0;
    J(5, 4) = 0;
    J(5, 5) = s + z[104]*(z[108]*z[22] + z[137] + z[262]) - z[11] + z[133]*(z[142] + 2*z[18]*z[94] + z[282]) + z[15] + z[2]*(z[104]*z[302] + z[317]) + z[333]*z[43];
    J(5, 6) = Ixz*x[0 + 5] - Iyy*x[2 + 5] + Iyz*z[23] + z[10] + z[104]*(z[130]*z[16] + z[283] + z[99]) + z[112]*z[24]*z[295] + z[112]*z[24]*(z[124]*z[18] + z[125]) + z[292];
    J(5, 7) = -Ixy*x[0 + 5] - Iyz*z[20] + Izz*x[1 + 5] + z[1]*(z[24]*z[302]*z[96] + z[317]) - z[12] + z[133]*(z[140] + z[193] + z[266]) + z[24]*z[96]*(x[0 + 5]*z[122] - z[100]) - z[52]*(z[24]*z[269]*z[96] + z[314]);
    J(6, 0) = 0;
    J(6, 1) = m*z[231]*(x[0 + 5]*z[195] + x[0 + 5]*z[261] - x[1 + 5]*z[186] - x[1 + 5]*z[284]) + m*z[235]*(x[1 + 5]*(x[1 + 5]*z[148] + z[98]) + z[17]*z[261] + z[17]*(-x[2 + 5]*z[148] - z[128]) + z[287]) + z[131]*(x[1 + 5]*(x[1 + 5]*z[231] + z[228]) + z[17]*(-x[2 + 5]*z[231] + z[239]) + z[355]) + z[150]*(x[0 + 5]*z[310] + z[16]*z[305] + z[354]) - z[21]*(z[0]*z[235]*z[40] + z[112]*z[47] + z[148]*z[32]*z[5] + z[231]*z[55]) + z[331]*z[68];
    J(6, 2) = m*z[212]*(x[1 + 5]*(x[1 + 5]*z[149] + z[98]) + z[17]*(-x[2 + 5]*z[149] + z[141]) + z[265] + z[287]) + m*z[215]*(x[0 + 5]*z[196] + z[16]*z[186] + z[327]) + z[131]*(x[1 + 5]*z[271] + x[1 + 5]*z[346] + z[17]*z[325] + z[17]*(-x[2 + 5]*z[215] + z[224])) + z[152]*(x[0 + 5]*z[296] + z[353]) - z[21]*(-z[112]*z[55] - z[149]*z[49] - z[212]*z[49] + z[215]*z[55]) + z[58]*(z[152]*z[303] + z[316]) + z[65]*(z[152]*z[271] + z[294]);
    J(6, 3) = 0;
    J(6, 4) = 0;
    J(6, 5) = Ixx*x[2 + 5] - Ixz*z[22] - Iyz*x[1 + 5] - z[10] + z[131]*(x[1 + 5]*z[94] + z[125]) + z[150]*(z[130]*z[18] + z[151] + z[262]) + z[2]*z[340] + z[331]*z[43];
    J(6, 6) = s + z[11] + z[131]*(z[148]*z[23] + z[285] + z[98]) - z[14] + z[150]*(-z[112]*z[23] + z[116] - z[266] + z[99]) + z[331];
    J(6, 7) = Ixy*x[1 + 5] + Ixz*z[20] - Izz*x[0 + 5] + z[1]*z[340] + z[13] + z[131]*(z[148]*z[20] + z[278]) + z[150]*(-z[100] + z[148]*z[18]) - z[331]*z[52];
    J(7, 0) = 0;
    J(7, 1) = m*z[222]*(x[1 + 5]*z[191] + z[17]*(z[137] + z[141]) + z[265] + z[286]) + m*z[230]*(-x[0 + 5]*z[191] - x[0 + 5]*z[280] + x[2 + 5]*(z[129] - z[99]) + z[288]) + z[147]*(x[2 + 5]*z[305] + z[18]*z[302] + z[18]*z[349] + z[350]) - z[21]*(z[0]*z[122] + z[0]*z[222]*z[40] + z[230]*z[5] + z[40]*z[5]*z[94]) + z[24]*z[94]*(x[1 + 5]*z[302] + z[17]*(z[237] + z[239]) + z[355]) + z[68]*(z[24]*z[268]*z[94] + z[313]);
    J(7, 2) = m*z[211]*(x[2 + 5]*z[186] + z[18]*z[187] + z[18]*z[280] + z[288]) + z[147]*(x[2 + 5]*z[344] + z[18]*z[346] + z[18]*(x[0 + 5]*z[48]*z[73]*z[89] + z[16]*z[211]) + z[273]) + z[162]*(x[1 + 5]*(x[0 + 5]*(e - z[7]*z[85]) - z[142]) - x[2 + 5]*(z[110] + z[171]*z[7]*z[86]) - x[2 + 5]*(-z[128] + z[137]) + z[286]) - z[21]*(z[211]*z[5] + z[40]*z[77]*z[97] + z[55]*z[94]) + z[24]*z[94]*(x[1 + 5]*z[274] + x[1 + 5]*z[346] + z[17]*z[295] + z[17]*z[325]) + z[58]*(z[24]*z[300]*z[94] + z[318]) + z[65]*(z[147]*z[295] + z[24]*z[270]*z[94]);
    J(7, 3) = 0;
    J(7, 4) = 0;
    J(7, 5) = -Ixx*x[1 + 5] + Ixy*z[22] + Iyz*x[2 + 5] + z[102]*(z[108]*z[17] + z[16]*z[96]) + z[12] + z[147]*(z[142] + z[22]*z[96] + z[282]) + z[2]*(z[102]*z[300] + z[318]) + z[43]*(z[291] + z[313]);
    J(7, 6) = -Ixy*z[23] - Ixz*x[2 + 5] + Iyy*x[0 + 5] + z[102]*(z[146]*z[23] + z[18]*z[96] + z[285]) - z[13] + z[146]*z[24]*z[293] + z[146]*z[24]*(z[125] + z[146]*z[18]) + z[291];
    J(7, 7) = s + z[1]*(z[102]*z[300] + z[318]) + z[102]*(2*z[122]*z[17] + z[278]) + z[14] + z[147]*(z[129] + z[193] + z[266]) - z[15] - z[52]*(z[291] + z[313]);

    // Entries of Mass matrix
    M_dyn(0, 0) = -Ixx - z[104]*z[94] - z[108]*z[133];
    M_dyn(0, 1) = z[192];
    M_dyn(0, 2) = z[194];
    M_dyn(1, 0) = z[192];
    M_dyn(1, 1) = -Iyy - m*pow(z[124], 2) - z[112]*z[131];
    M_dyn(1, 2) = z[188];
    M_dyn(2, 0) = z[194];
    M_dyn(2, 1) = z[188];
    M_dyn(2, 2) = -Izz - z[102]*z[96] - z[122]*z[147];

    J.bottomRows(3) = M_dyn.fullPivLu().solve(J.bottomRows(3));

    for (int i = 0; i < 8; ++i)
      for (int j = 0; j < 8; ++j)
        dfdx[8*i + j] = J(i, j);
    
    return GSL_SUCCESS;
  }

  void outputs(simdata *sd, parameters *p)
  {
    // Compute dxdt
    ode(sd->t, sd->x, sd->dxdt, static_cast<void *>(p));

    // Pointers to state and state derivative vectors
    const double * const x = sd->x;
    const double * const dxdt = sd->dxdt;

    // Parameters
    const double a = p->a,
                        b = p->b,
                        c = p->c,
                        d = p->d,
                        e = p->e,
                        f = p->f,
                        m = p->m,
                        Ixx = p->Ixx,
                        Iyy = p->Iyy,
                        Izz = p->Izz,
                        Ixy = p->Ixy,
                        Iyz = p->Iyz,
                        Ixz = p->Ixz,
                        g = p->g;
    double z[90];
    
    // Output quantites (evaluated at each output time-step)
    z[0] = cos(x[2]);
    z[1] = pow(b, 2);
    z[2] = sin(x[1]);
    z[3] = pow(a, 2);
    z[4] = cos(x[1]);
    z[5] = sin(x[2]);
    z[6] = pow(c, 2);
    z[7] = 0.5*m;
    z[8] = g*m;
    z[9] = m*dxdt[1 + 5];
    z[10] = -x[2 + 5];
    z[11] = m*dxdt[0 + 5];
    z[12] = m*dxdt[2 + 5];
    z[13] = -dxdt[1];
    z[14] = -x[0 + 5];
    z[15] = -x[1 + 5];
    z[16] = pow(z[4], 2);
    z[17] = pow(z[0], 2);
    z[18] = pow(z[5], 2);
    z[19] = m*z[0];
    z[20] = dxdt[1]*z[2];
    z[21] = z[0]*z[4];
    z[22] = z[2]*z[5];
    z[23] = z[4]*z[5];
    z[24] = z[0]*z[6];
    z[25] = z[18]*z[3];
    z[26] = z[17]*z[6];
    z[27] = dxdt[2]*z[16]*z[5];
    z[28] = 0.5*x[2 + 5]*(Ixz*x[0 + 5] + Iyz*x[1 + 5] + Izz*x[2 + 5]);
    z[29] = 0.5*x[0 + 5]*(Ixx*x[0 + 5] + Ixy*x[1 + 5] + Ixz*x[2 + 5]);
    z[30] = 0.5*x[1 + 5]*(Ixy*x[0 + 5] + Iyy*x[1 + 5] + Iyz*x[2 + 5]);
    z[31] = z[1]*pow(z[2], 2) + z[16]*z[25] + z[16]*z[26];
    z[32] = pow(z[31], -1.0/2.0);
    z[33] = pow(z[32], 3);
    z[34] = z[28] + z[29] + z[30];
    z[35] = z[3]*z[32];
    z[36] = z[1]*z[32];
    z[37] = z[32]*z[6];
    z[38] = e - z[2]*z[36];
    z[39] = z[2]*z[36];
    z[40] = -e + z[39];
    z[41] = x[0 + 5]*z[38];
    z[42] = f - z[21]*z[37];
    z[43] = -d - z[23]*z[35];
    z[44] = x[1 + 5]*z[42];
    z[45] = dxdt[2]*z[23]*z[37];
    z[46] = z[20]*z[24]*z[32];
    z[47] = d + z[23]*z[35];
    z[48] = x[0 + 5]*z[42];
    z[49] = -z[42];
    z[50] = x[1 + 5]*z[43];
    z[51] = z[14]*z[42];
    z[52] = x[2 + 5]*z[47];
    z[53] = x[1 + 5]*z[47];
    z[54] = -z[43];
    z[55] = z[10]*z[43];
    z[56] = z[54]*z[9];
    z[57] = -z[0]*z[27]*z[3] - z[1]*z[20]*z[4] + z[20]*z[25]*z[4] + z[20]*z[26]*z[4] + z[24]*z[27];
    z[58] = -x[2 + 5]*z[38] + z[44];
    z[59] = x[2 + 5]*z[40] + z[44];
    z[60] = z[10]*z[38] + z[44];
    z[61] = z[14]*z[40] + z[50];
    z[62] = z[41] + z[50];
    z[63] = pow(z[60], 2);
    z[64] = z[12]*z[38] + z[49]*z[9];
    z[65] = x[1 + 5]*z[62];
    z[66] = z[45] + z[46];
    z[67] = -z[48] + z[55];
    z[68] = z[51] + z[55];
    z[69] = -z[11]*z[38] + z[56];
    z[70] = x[0 + 5]*z[68];
    z[71] = z[11]*z[42] + z[12]*z[43];
    z[72] = z[10]*(x[2 + 5]*z[54] + z[51]);
    z[73] = z[33]*z[57];
    z[74] = -z[73];
    z[75] = z[6]*z[73];
    z[76] = z[1]*z[2]*z[73];
    z[77] = z[2]*z[38] + z[21]*(f - z[24]*z[32]*z[4]) - z[23]*z[47];
    z[78] = -dxdt[1]*z[36]*z[4] - z[76];
    z[79] = z[1]*z[2]*z[74] + z[13]*z[36]*z[4];
    z[80] = x[0 + 5]*z[79];
    z[81] = z[10]*z[79];
    z[82] = -z[21]*z[75] + z[66];
    z[83] = dxdt[2]*z[21]*z[35] + z[13]*z[22]*z[35] + z[23]*z[3]*z[73];
    z[84] = x[1 + 5]*z[82];
    z[85] = x[2 + 5]*z[83];
    z[86] = x[1 + 5]*z[83];
    z[87] = z[15]*z[83];
    z[88] = z[72] + z[81] + z[84];
    z[89] = z[70] + z[80] + z[87];

    // Contact forces
    sd->CF[0] = m*z[5]*(x[0 + 5]*z[67] + x[0 + 5]*z[78] - x[1 + 5]*z[60] - z[86]) - z[0]*z[64] + z[19]*(x[1 + 5]*(z[21]*z[6]*z[74] + z[66]) + z[10]*z[78] + z[10]*(x[2 + 5]*z[54] - z[48]) + z[65]) - z[5]*z[69];
    sd->CF[1] = m*z[22]*(x[1 + 5]*z[61] + z[88]) + m*z[4]*(x[2 + 5]*z[59] + z[14]*z[82] + z[14]*(-x[0 + 5]*z[40] + z[50]) + z[85]) + z[0]*z[2]*(z[11]*z[40] + z[56]) - z[19]*z[2]*(z[15]*z[59] + z[89]) - z[22]*(-z[12]*z[40] - z[42]*z[9]) - z[4]*z[71];
    sd->CF[2] = m*z[2]*(-x[0 + 5]*z[62] - x[0 + 5]*z[82] + x[2 + 5]*z[58] + z[85]) - m*z[23]*(x[1 + 5]*(x[1 + 5]*(-d - z[35]*z[4]*z[5]) + z[41]) + x[1 + 5]*(dxdt[2]*z[37]*z[4]*z[5] - z[21]*z[75] + z[46]) + z[10]*(-x[2 + 5]*(-d - z[35]*z[4]*z[5]) + z[51]) + z[81]) + z[19]*z[4]*(z[15]*z[58] + z[89]) - z[2]*z[71] - z[21]*z[69] + z[23]*z[64] - z[8];

    // Mechanical energy
    sd->ke = z[34] + z[7]*(z[63] + pow(z[41] - z[53], 2) + pow(-z[48] + z[52], 2));
    sd->pe = -z[77]*z[8];
    sd->te = sd->ke + sd->pe;
    // Tilt of Rattleback with respect to vertical
    sd->delta = acos(z[21]);

    // State time derivatives
    for (int i = 0; i < 8; ++i)
      sd->dxdt[i] = dxdt[i];
  }

  void ode_eq(const double roll_pitch_yawrate[], double f_dyn_eq[], void *params)
  {
    parameters *p = static_cast<parameters *>(params);
    // Parameters
    const double a = p->a,
                        b = p->b,
                        c = p->c,
                        d = p->d,
                        e = p->e,
                        f = p->f,
                        m = p->m,
                        Ixx = p->Ixx,
                        Iyy = p->Iyy,
                        Izz = p->Izz,
                        Ixy = p->Ixy,
                        Iyz = p->Iyz,
                        Ixz = p->Ixz,
                        g = p->g,
                        s = p->s;
    static double z[38];

    // Intermediate quantities needed for dynamic equilibrium
    z[0] = sin(roll_pitch_yawrate[0]);
    z[1] = cos(roll_pitch_yawrate[0]);
    z[2] = cos(roll_pitch_yawrate[1]);
    z[3] = sin(roll_pitch_yawrate[1]);
    z[4] = pow(b, 2);
    z[5] = pow(a, 2);
    z[6] = pow(c, 2);
    z[7] = g*m;
    z[8] = -m;
    z[9] = pow(z[1], 2);
    z[10] = roll_pitch_yawrate[2]*z[1];
    z[11] = -z[3];
    z[12] = roll_pitch_yawrate[2]*z[0];
    z[13] = -z[2];
    z[14] = z[1]*z[3];
    z[15] = z[1]*z[2];
    z[16] = -Ixz*z[10]*z[3] + Iyz*z[12] + Izz*z[10]*z[2];
    z[17] = -Ixy*z[10]*z[3] + Iyy*z[12] + Iyz*z[10]*z[2];
    z[18] = -Ixx*z[10]*z[3] + Ixy*z[12] + Ixz*z[10]*z[2];
    z[19] = pow(pow(z[0], 2)*z[4] + pow(z[2], 2)*z[6]*z[9] + pow(z[3], 2)*z[5]*z[9], -1.0/2.0);
    z[20] = e - z[0]*z[19]*z[4];
    z[21] = f - z[15]*z[19]*z[6];
    z[22] = z[1]*z[20];
    z[23] = z[2]*z[20];
    z[24] = d + z[14]*z[19]*z[5];
    z[25] = -z[21];
    z[26] = m*z[21];
    z[27] = -z[24];
    z[28] = z[21]*z[3];
    z[29] = z[12]*z[21];
    z[30] = z[10]*z[11]*z[20];
    z[31] = z[10]*z[28];
    z[32] = -z[10]*z[23] + z[29];
    z[33] = z[10]*z[13]*z[20] + z[29];
    z[34] = -z[10]*z[20]*z[3] - z[12]*z[24];
    z[35] = z[10]*z[2]*z[24] + z[31];
    z[36] = z[10]*z[13]*z[27] + z[31];
    z[37] = -z[36];

    // Right hand side of dynamic equations under equilibrium conditions
    f_dyn_eq[0] = -m*z[20]*(z[10]*z[3]*(roll_pitch_yawrate[2]*z[14]*z[21] + roll_pitch_yawrate[2]*z[15]*z[24]) + z[12]*(roll_pitch_yawrate[2]*z[0]*z[21] - z[10]*z[23])) - s*z[10]*z[3] - z[10]*z[2]*(-Ixy*roll_pitch_yawrate[2]*z[14] + Iyy*z[12] + Iyz*roll_pitch_yawrate[2]*z[15]) + z[12]*(-Ixz*z[10]*z[3] + Iyz*roll_pitch_yawrate[2]*z[0] + Izz*z[10]*z[2]) + z[21]*z[8]*(z[10]*z[2]*(-roll_pitch_yawrate[2]*z[15]*z[20] + z[29]) + z[10]*z[3]*(roll_pitch_yawrate[2]*z[11]*z[22] - z[12]*z[24])) - z[7]*(z[0]*z[25] + z[15]*z[20]);
    f_dyn_eq[1] = m*z[27]*(z[10]*z[11]*(roll_pitch_yawrate[2]*z[1]*z[13]*z[27] + roll_pitch_yawrate[2]*z[14]*z[21]) - z[12]*(roll_pitch_yawrate[2]*z[0]*z[21] + z[10]*z[13]*z[20])) + s*z[12] + z[10]*z[2]*(-Ixx*roll_pitch_yawrate[2]*z[14] + Ixy*z[12] + Ixz*roll_pitch_yawrate[2]*z[15]) + z[10]*z[3]*(-Ixz*roll_pitch_yawrate[2]*z[14] + Iyz*z[12] + Izz*roll_pitch_yawrate[2]*z[15]) + z[26]*(z[10]*z[13]*(roll_pitch_yawrate[2]*z[14]*z[21] - roll_pitch_yawrate[2]*z[15]*z[27]) + z[12]*(roll_pitch_yawrate[2]*z[0]*z[27] + z[30])) - z[7]*(-z[14]*z[21] + z[15]*z[27]);
    f_dyn_eq[2] = m*z[24]*(z[10]*z[2]*(roll_pitch_yawrate[2]*z[13]*z[22] + z[29]) + z[10]*z[3]*(-roll_pitch_yawrate[2]*z[14]*z[20] - z[12]*z[24])) + s*z[10]*z[2] - z[10]*z[3]*(-Ixy*roll_pitch_yawrate[2]*z[14] + Iyy*z[12] + Iyz*roll_pitch_yawrate[2]*z[15]) - z[12]*(-Ixx*z[10]*z[3] + Ixy*roll_pitch_yawrate[2]*z[0] + Ixz*z[10]*z[2]) + z[20]*z[8]*(-z[10]*z[2]*(roll_pitch_yawrate[2]*z[14]*z[21] + roll_pitch_yawrate[2]*z[15]*z[24]) + z[12]*(-roll_pitch_yawrate[2]*z[0]*z[24] - z[10]*z[20]*z[3])) - z[7]*(z[0]*z[24] + z[14]*z[20]);
  }
}}}}
